/**
 *   Copyright (C) 2021 All rights reserved.
 *
 *   FileName      ：cordic.cpp
 *   Author        ：hpy
 *   Email         ：yuan_hp@qq.com
 *   Date          ：2021年04月17日
 *   Description   ： https://blog.csdn.net/u010712012/article/details/77755567
 */

#include <bits/stdc++.h>
using namespace std;

#define PI 3.1415926

const double atanTable[] = {0.785398, 0.463648, 0.244979, 0.124355, 0.0624188, 0.0312398, 0.0156237, 0.00781234, 0.00390623, 0.00195312, 0.000976562, 0.000488281, 0.000244141, 0.00012207, 6.10352e-05, 3.05176e-05};

//------------------ 分析原理 ----------------------------------
void MakeDtheta(){
	double s = 1;
	for( int i = 0; i < 16 ; i++ ){
		double a = pow(2,-i) ; //量化值
		double b = atan(a);  //角度
		double c = cos(b) ; //

		s = s * c ;
		cout << i<< " " <<180/PI*b<<" " << c << " " << s << " " << 1/s << endl;
	}
}

//-------------- 生成16次迭代的cordic的atan表 -------------------
void MakeCordicAtanTable(){
		cout << "const double atanTable[] = {" ;
	for(int i = 0 ; i < 16 ; i ++ ){
		if(i<15){ 
			cout <<atan(pow(2,-i))<< ", " ;  //弧度
		} else {
			cout <<atan(pow(2,-i))<< "};" <<endl ;  //弧度
		}
	}
}

//------------------ cordic 计算sin和cos值 tan值 ----------------
void cordicSinCos(double t ){  //弧度输入  -99.7度 ～ 99.7度
	double x = 0.607253;  //初始值
	double y = 0;  //初始值
	double xi,yi,d;
	double z = t;
	for(int i = 0;i<16 ; i++){ //迭代16次
		d = (z>=0) ? 1 : -1;
		xi = x - d * y * pow(2,-i) ;
		yi = y + d * x * pow(2,-i) ;
		z  = z - d * atan( pow(2,-i)) ; //可以使用查表实现

		x = xi;
		y = yi;
	}
	cout<< "cos = " << x << " " << cos(t) << endl ;
	cout<< "sin = " << y << " " << sin(t) << endl ;
	cout<< "tan = " << y/x << " " << tan(t) << endl ;

	//cout << x << " "<< y <<endl ;
}

//------------------ 整数量化 cordic 计算sin和cos值 tan值 ----------------
#define SC_MAX 8192
void cordicSinCos_int(double t ){  //弧度输入  -99.7度 ～ 99.7度
	int x = int(SC_MAX *0.607253);  //初始值
	int y = 0;  //初始值
	int xi,yi,d;
	int z = t * SC_MAX;
	for(int i = 0;i<16 ; i++){ //迭代16次
		d = (z>=0) ? 1 : -1;
		xi = x - d * y / (1<<i) ;
		yi = y + d * x / (1<<i) ;
		z  = z - d * atanTable[i] * SC_MAX; //查表

		x = xi;
		y = yi; 
	}
	cout<< "cos = " << double(x/65536.0 )<< " " << cos(t) << endl ;
	cout<< "sin = " << double(y/65536.0)<< " " << sin(t) << endl ;
	cout<< "tan = " << double(1.0*y/x) << " " << tan(t) << endl ;

	
}


// ------------------  cordic 计算atan值  ------------------
void cordicAtan(double x0, double y0 ){  //弧度输入 坐标(x,y) tan = y/x
	double x = x0;  //初始值
	double y = y0;  //初始值

	double k = 0.607253;
	double xi,yi,d;
	double z = 0;
	for(int i = 0;i<16 ; i++){ //迭代16次
		d = (y>=0) ? -1 : 1;
		xi = x - d * y * pow(2,-i) ;
		yi = y + d * x * pow(2,-i) ;
		z  = z - d * atan( pow(2,-i)) ; 

		x = xi;
		y = yi;
	}
	// cout<< "cos = " << x << " " << cos(t) << endl ;
	// cout<< "（度数）atan = " << 180/PI* z<< " " << 180/PI* atan(y0/x0) << endl ;
	// cout << x << endl;
	// cout << y << endl;
	// cout << z << endl;
	// cout << k*sqrt(2) << endl;

	//cout << 180/PI* z<< " "<<180/PI* atan(1.0*y0/x0)<<endl ;

	//cout << x0 << " " << y0 << " " << 180/PI* z << " " << 180/PI* atan(1.0*y0/x0) <<endl;
	
}

// ------------------  整数量化 cordic 计算atan值  ------------------
void cordicAtan_int(int x0, int y0 ){  //弧度输入 坐标(x,y) tan = y/x
	long x = x0*4096;  //初始值
	long y = y0*4096;  //初始值

	long xi,yi,d;
	long z = 0;
	for(int i = 0;i<16 ; i++){ //迭代16次
		d = (y>=0) ? -1 : 1;
		xi = x - d * y /(1<<i) ;
		yi = y + d * x / (1<<i) ;
		z  = z - d * atanTable[i]* SC_MAX;  

		x = xi;
		y = yi;
	}
	// cout<< "cos = " << x << " " << cos(t) << endl ;
	// cout<< "（度数）atan = " << 180/PI* (1.0*z/SC_MAX)<< " " << 180/PI* atan(y0/x0) << endl ;
	// cout << x << endl;
	// cout << y << endl;
	// cout << (1.0*z/SC_MAX) << endl;

	//cout << 180/PI* 1.0*z/SC_MAX<< " "<<180/PI* atan(1.0*y0/x0)<<endl ;
	//cout << 180/PI* (1.0*z/SC_MAX) - 180/PI* atan(1.0*y0/x0)<<endl ;

	//cout << 180/PI* 1.0*z/SC_MAX<< " "<<180/PI* atan(1.0*y0/x0) <<endl ;

	cout << x0 << " " << y0 << " " << 180/PI* z / SC_MAX  << " " << 180/PI* atan(1.0*y0/x0) <<endl;
}


int main(int argc, char* argv[])
{
	//MakeDtheta();  //cordic理论误差分析
	//--------------------------------------------------------------------
	//cordicSinCos(-PI/2);  //计算sin和cos值 tan值，直接直接实现，没有使用整数量化
	//cordicSinCos_int(-PI/2);

	//--------------------------------------------------------------------
	//cordicAtan(1,2);  //计算tan(y/x)
	 //cordicAtan_int(65533,1000);
 
 
	for(int i = 1 ; i < 6000;i+=200){
		for(int j = 6000 ; j >1 ;j-=200){
			cordicAtan_int(i,j);
		}
	}
	

	return 0;
}

